#include <stdlib.h>
#include "crvm.h"

#define SET_STATUS(flag,val) \
	if (val) \
	*sfr |= STATUS_##flag; \
else \
	*sfr &= ~ STATUS_##flag;


unsigned long cyclenumble = 0;
unsigned long instruction_numble = 0;
byte  cycperinstum = 1;
typedef int (*DECODE)(word opcode);
StateType	crstate;
bool interflag;
DECODE decodetable[9];

void instruction_incr(void)
{
	instruction_numble++;
	cyclenumble += instruction_numble*cycperinstum;
	if(++*pcl == 0)
	{
		pc += 0x100;
		pc &= 0x7ff;
	}
	pc = (pc & 0xff00) | ((romaddr)(*pcl)&0x00ff);
}

void op_NOP(word opcode)
{
}

void no_SLEEP(word opcode)
{
	crstate = STATE_SLEEP;
	SET_STATUS(SP,0)
}

void op_CLRWT(word opcode)
{
	*wtc = 0;
	SET_STATUS(SP,1)
}

void op_RETI(word opcode)
{
	pc = pop() & 0x7ff;
	*pcl = pc & 0xff;

	*adf &= ~ ADF_AINTF;
	interflag = 1;
}

void op_RET(word opcode)
{
	pc = pop() & 0x7ff;
	*pcl = pc & 0xff;
}

void op_CLRIF(word opcode)
{
	*adf &= ~ ADF_AINTF;
	interflag = 1;
}

void op_STDBY(word opcode)
{
	crstate = STATE_STOPPED;
	SET_STATUS(SP,0)
}

void op_CLRA(word opcode)
{
	acc = 0;
	SET_STATUS(Z,1)
}

void op_STAR(word opcode)
{
	byte reg;
	reg = opcode & 0x7f;
	write_reg_abs(reg,acc);
}

void op_CTRLR(word opcode)
{
	byte reg;

	reg = opcode & 0x7;
	if(opcode &(1<<3))
	{
		implicit_registers[reg]=acc;
	}
	else {
		acc = implicit_registers[reg];
	}	

}

void op_CLRR(word opcode)
{
	byte reg;
	reg = opcode & 0x7f;
	write_reg_abs(reg,0);
	SET_STATUS(Z,1)
}

void op_IORAR(word opcode)
{

	byte reg,result;
	reg = opcode & 0x7f;

	result =  read_reg_abs(reg) | acc;
	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}
	SET_STATUS(Z, result == 0)

}

void op_XORAR(word opcode)
{
	byte reg,result;
	reg = opcode & 0x7f;
	result =  read_reg_abs(reg) ^ acc;
	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}
	SET_STATUS(Z, result == 0)
}

void op_ANDAR(word opcode)
{
	byte reg,result;
	reg = opcode & 0x7f;
	result =  read_reg_abs(reg) & acc;
	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}
	SET_STATUS(Z, result == 0)
}

void op_ADDAR(word opcode)
{
	byte reg,regv;
	word result;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	result = (regv & 0xf) + (acc & 0xf);
	SET_STATUS(AC, result & 0x10)

		result = regv + acc;
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)

		if(opcode &(1<<7))
		{
			write_reg_abs (reg,(byte)result);
		}
		else 
		{
			acc = (byte)result;
		}
}

void op_SUBAR(word opcode)
{
	byte reg,regv;
	word result;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	result = (regv & 0xf) + ((acc & 0xf) ^ 0xf) + 1;
	SET_STATUS(AC, result & 0x10)
		result = regv + (acc ^ 0xff) + 1;
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)

		if(opcode &(1<<7))
		{
			write_reg_abs (reg,result);
		}
		else 
		{
			acc = result;
		}
}

void op_LDR(word opcode)
{
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	SET_STATUS(Z, regv == 0)
		if(opcode &(1<<7))
		{
			write_reg_abs (reg,regv);
		}
		else 
		{
			acc = regv;
		}
}




void op_COMR(word opcode)
{
	byte reg,regv;
	byte result;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	result = ~regv;
	SET_STATUS(Z, result == 0)
		if(opcode &(1<<7))
		{
			write_reg_abs (reg,result);
		}
		else 
		{
			acc = result;
		}
}

void op_DECR(word opcode)
{
	byte reg,regv;
	word result;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	result = regv -1;
	result &= 0x0ff;
	SET_STATUS(Z, result == 0)
		if(opcode &(1<<7))
		{
			write_reg_abs (reg,result);
		}
		else 
		{
			acc =result ;
		}
}

void op_DRSZ(word opcode)
{
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg) - 1;
	if(opcode &(1<<7))
	{
		write_reg_abs (reg,regv);
		if(!regv)
			instruction_incr();
	}
	else 
	{
		acc = regv;
		if(!acc)
			instruction_incr();
	}
}

void op_INCR(word opcode)
{
	byte reg;
	word result;
	reg = opcode & 0x7f;
	result = read_reg_abs(reg) +1;
	result &= 0x0ff;
	SET_STATUS(Z, result == 0)

		if(opcode &(1<<7))
		{
			write_reg_abs (reg,result);
		}
		else 
		{
			acc =result ;
		}
}

void op_IRSZ(word opcode)
{
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg) + 1;
	if(opcode &(1<<7))
	{
		write_reg_abs (reg,regv);
		if(!regv)
			instruction_incr();
	}
	else 
	{
		acc = regv;
		if(!acc)
			instruction_incr();
	}
}

void op_SWAPR(word opcode)
{
	byte reg,regv;
	byte tmp;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	if(opcode &(1<<7))
	{
		tmp = (regv & 0x0f) <<4;
		tmp = (regv & 0xf0) >>4 | tmp;
		write_reg_abs (reg,tmp);
	}
	else 
	{
		tmp = (regv & 0x0f) <<4;
		tmp = (regv & 0xf0) >>4 | tmp;
		acc = tmp;
	}
}


void op_RLR(word opcode)
{
	byte reg,regv;
	word result;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	result = (regv ) <<1;
	result |= (*sfr & STATUS_C);
	SET_STATUS(C, regv & 0x80)
		result &= 0x0ff;


	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}
}

void op_RRR(word opcode)
{
	byte reg,regv;
	word result;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	result = (regv ) >>1;
	result |= ((*sfr & STATUS_C))<<7;
	SET_STATUS(C, regv& 0x01)
		result &= 0x0ff;

	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}
}


void op_BCR(word opcode)
{
	byte reg,regv;
	byte bit;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	bit = (opcode >> 7) & 0x07;
	write_reg_abs (reg,regv & ~(1<<bit));
}

void op_BSR(word opcode)
{
	byte reg,regv;
	byte bit;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	bit = (opcode >> 7) & 0x07;
	write_reg_abs (reg,regv | (1<<bit));

}



void op_BTRSC(word opcode)
{
	byte reg,regv;
	byte bit;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	bit = (opcode >> 7) & 0x07;

	if(!( regv & (1<<bit)))
	{
		instruction_incr();
	}
}

void op_BTRSS(word opcode)
{
	byte reg,regv;
	byte bit;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);
	bit = (opcode >> 7) & 0x07;

	if( regv & (1<<bit))
	{
		instruction_incr();
	}
}

void op_JUMP(word opcode)
{
	pc = opcode & 0x7ff;
	*pcl = (char)opcode & 0xff;
}

void op_CALL(word opcode)
{
	push(pc);

	pc = opcode & 0x7ff;
	*pcl = (char)opcode & 0xff;

}

void op_ADCAR(word opcode)
{
	word result;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);


	result = (regv & 0xf) + (acc & 0xf) +(*sfr & STATUS_C);
	SET_STATUS(AC, result & 0x10)

		result = regv + acc +(*sfr & STATUS_C);
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)

		if(opcode &(1<<7))
		{
			write_reg_abs (reg,result);
		}
		else 
		{
			acc = result;
		}
}


void op_SBCAR(word opcode)
{
	word result;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	result = (regv & 0xf) + ((acc & 0xf) ^ 0xf) + 1 -  +(*sfr & STATUS_C);
	SET_STATUS(AC, result & 0x10)
		result = regv + (acc ^ 0xff) + 1 -  +(*sfr & STATUS_C);
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)

		if(opcode &(1<<7))
		{
			write_reg_abs (reg,result);
		}
		else 
		{
			acc = result;
		}
}

void op_SRR(word opcode)
{
	word result;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);


	result = ( regv ) >>1;
	SET_STATUS(C, regv & 0x01)
		result &= 0x0ff;

	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}

}

void op_SLR(word opcode)
{
	word result;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	result = (regv ) <<1;
	SET_STATUS(C, regv & 0x80)
		result &= 0x0ff;

	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}
}

void op_DAR(word opcode)
{
	word result;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	result = regv;
	if( result >9  | (*sfr & STATUS_AC))
	{
		result = (regv &0x0f) + 6;
		SET_STATUS(AC, result & 0x10)
			result = regv +  6;
	}

	if( (result >>4) > 9  | (*sfr & STATUS_C))
		result = regv + 6<<4;

	SET_STATUS(C, result & 0x100)	
		result &= 0xff;
	if(opcode &(1<<7))
	{
		write_reg_abs (reg,result);
	}
	else 
	{
		acc = result;
	}
}

void op_XCHAR(word opcode)
{
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	write_reg_abs (reg,acc);
	acc = regv;

}

void op_CMPAR(word opcode)
{
	word result;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	result = (regv & 0xf) + ((acc & 0xf) ^ 0xf) + 1;
	SET_STATUS(AC, result & 0x10)
		result = regv + (acc ^ 0xff) + 1;
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)
}

void op_BCTR(word opcode)
{
	byte bit;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	bit = (opcode >> 5) & 0x07;
	if(*sfr &STATUS_C)
		write_reg_abs (reg,regv | (1<<bit));
	else write_reg_abs (reg, regv & ~(1<<bit));

}

void op_BRTC(word opcode)
{
	byte bit;
	byte reg,regv;
	reg = opcode & 0x7f;
	regv = read_reg_abs(reg);

	bit = (opcode >> 5) & 0x07;

	if(regv &(1<<bit))
		*sfr = *sfr | STATUS_C;
	else 	*sfr = *sfr & (~STATUS_C);

}

void op_LDIA(word opcode)
{
	acc = opcode & 0xff;
}

void op_CMPIA(word opcode)
{
	word result;
	byte k;
	k = opcode & 0xff;
	result = k + (acc ^ 0xff) + 1;
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)
}

void op_IORIA(word opcode)
{
	byte k;
	k = opcode & 0xff;
	acc = k | acc;
	SET_STATUS(Z, acc == 0)
}

void op_XORIA(word opcode)
{
	byte k;
	k = opcode & 0xff;
	acc = k ^ acc;
	SET_STATUS(Z, acc == 0)
}

void op_RTIA(word opcode)
{
	acc = opcode & 0xff;
	pc = pop() & 0x7ff;
	*pcl = pc & 0xff;
}

void op_ADCIA(word opcode)
{
	byte k;
	word result;
	k = opcode & 0xff;
	result = (k & 0xf) + (acc & 0xf) +(*sfr & STATUS_C);
	SET_STATUS(AC, result & 0x10)

		result = k + acc +(*sfr & STATUS_C);
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)

		acc = result;

}

void op_SBCIA(word opcode)
{
	byte k;
	word result;
	k = opcode & 0xff;
	result = (k & 0xf) + ((acc & 0xf) ^ 0xf) + 1 -  +(*sfr & STATUS_C);
	SET_STATUS(AC, result & 0x10)
		result = k + (acc ^ 0xff) + 1 -  +(*sfr & STATUS_C);
	SET_STATUS(C, result & 0x100)
		result &= 0xff;
	SET_STATUS(Z, result == 0)

		acc = result;
}

void op_ANDIA(word opcode)
{
	byte k;
	k = opcode & 0xff;
	acc = k & acc;
	SET_STATUS(Z, acc == 0)
}

void decode(word opcode)
{
	romaddr decode;
	instruction_incr();
	opcode &= 0x3FFF;
	if(opcode == 0 )
		op_NOP(opcode);
	else if(opcode == 2 )
		no_SLEEP(opcode);
	else if(opcode == 3 )
		op_CLRWT(opcode);
	else if(opcode == 4 )
		op_RETI(opcode);
	else if(opcode == 5 )
		op_RET(opcode);
	else if(opcode == 6 )
		op_CLRIF(opcode);
	else if(opcode == 7 )
		op_STDBY(opcode);
	else if(opcode == 8 )
		op_CLRA(opcode);

	decode =  opcode & 0x3F00;
	switch(decode>>8)
	{
	case 0x38:
		op_LDIA(opcode);
		break;
	case 0x39:
		op_CMPIA(opcode);
		break;
	case 0x3a:
		op_IORIA(opcode);
		break;
	case 0x3b:
		op_XORIA(opcode);
		break;
	case 0x3c:
		op_RTIA(opcode);
		break;
	case 0x3d:
		op_ADCIA(opcode);
		break;
	case 0x3e:
		op_SBCIA(opcode);
		break;
	case 0x3f:
		op_ANDIA(opcode);
		break;
	default:
		break;
	}

	decode =  opcode & 0x3c00;
	switch(decode>>10)
	{
	case 0x02:
		op_BCR(opcode);
		break;

	case 0x03:
		op_BTRSC(opcode);
		break;

	case 0x06:
		op_BSR(opcode);
		break;

	case 0x07:
		op_BTRSS(opcode);
		break;
	case 0x08:
	case 0x09:
		op_JUMP(opcode);
		break;
	case 0x0a:
	case 0x0b:
		op_CALL(opcode);
		break;
	default:
		break;
	}	
	decode =  opcode & 0x3f80;
	switch(decode>>7)
	{
	case 1:
		op_STAR(opcode);
		break;

	case 2:
		op_CTRLR(opcode);
		break;
	case 3:
		op_CLRR(opcode);
		break;
	case 4:
	case 5:
		op_IORAR(opcode);
		break;
	case 6:
	case 7:
		op_XORAR(opcode);
		break;

	case 8:
	case 9:
		op_ANDAR(opcode);
		break;
	case 0x0a:
	case 0x0b:
		op_ADDAR(opcode);
		break;
	case 0xc:
	case 0x0d:
		op_SUBAR(opcode);
		break;
	case 0x0e:
	case 0x0f:
		op_LDR(opcode);
		break;
	case 0x20:
	case 0x21:
		op_COMR(opcode);
		break;
	case 0x22:
	case 0x23:
		op_DECR(opcode);
		break;
	case 0x24:
	case 0x25:
		op_DRSZ(opcode);
		break;
	case 0x26:
	case 0x27:
		op_INCR(opcode);
		break;
	case 0x28:
	case 0x29:
		op_IRSZ(opcode);
		break;

	case 0x2a:
	case 0x2b:
		op_SWAPR(opcode);
		break;

	case 0x2c:
	case 0x2d:
		op_RLR(opcode);
		break;

	case 0x2e:
	case 0x2f:
		op_RRR(opcode);
		break;

	case 0x60:
	case 0x61:
		op_ADCAR(opcode);
		break;

	case 0x62:
	case 0x63:
		op_SBCAR(opcode);
		break;

	case 0x64:
	case 0x65:
		op_SRR(opcode);
		break;

	case 0x66:
	case 0x67:
		op_SLR(opcode);
		break;

	case 0x68:
	case 0x69:
		op_DAR(opcode);
		break;

	case 0x6a:
		op_XCHAR(opcode);
		break;

	case 0x6b:
		op_CMPAR(opcode);
		break;

	case 0x6c:
	case 0x6d:
		op_BCTR(opcode);
		break;

	case 0x6e:
	case 0x6f:
		op_BRTC(opcode);
		break;
	default:		
		break;
	}
}

